/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, 2013, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Eitan Marder-Eppstein
 *         David V. Lu!!
 *********************************************************************/
#include <costmap_2d/costmap_2d.h>
#include <cstdio>

using namespace std;

namespace costmap_2d
{
Costmap2D::Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, double resolution,
                     double origin_x, double origin_y, unsigned char default_value) :
        size_x_(cells_size_x), size_y_(cells_size_y), resolution_(resolution), origin_x_(origin_x),
        origin_y_(origin_y), costmap_(NULL), default_value_(default_value)
{
    access_ = new mutex_t();
    // create the costmap
    initMaps(size_x_, size_y_);
    resetMaps();
}

// just initialize everything to NULL by default
Costmap2D::Costmap2D() : size_x_(0), size_y_(0), resolution_(0.0), origin_x_(0.0),
                         origin_y_(0.0), costmap_(NULL) { access_ = new mutex_t(); }

Costmap2D::Costmap2D(const Costmap2D &map) : costmap_(NULL)
{
    access_ = new mutex_t();
    *this = map;
}

Costmap2D::~Costmap2D()
{
    deleteMaps();
    delete access_;
}

void Costmap2D::deleteMaps()
{
    // clean up data
    boost::unique_lock<mutex_t> lock(*access_);
    delete[] costmap_;
    costmap_ = NULL;
}

void Costmap2D::initMaps(unsigned int size_x, unsigned int size_y)
{
    boost::unique_lock<mutex_t> lock(*access_);
    delete[] costmap_;
    costmap_ = new unsigned char[size_x * size_y];
}

void Costmap2D::resizeMap(unsigned int size_x, unsigned int size_y, double resolution,
                          double origin_x, double origin_y)
{
    size_x_ = size_x;
    size_y_ = size_y;
    resolution_ = resolution;
    origin_x_ = origin_x;
    origin_y_ = origin_y;
    initMaps(size_x, size_y);
    // reset our maps to have no information
    resetMaps();
}

void Costmap2D::resetMaps()
{
    boost::unique_lock<mutex_t> lock(*access_);
    memset(costmap_, default_value_, size_x_ * size_y_ * sizeof(unsigned char));
}

void Costmap2D::resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn)
{
    boost::unique_lock<mutex_t> lock(*(access_));
    unsigned int len = xn - x0;
    for (unsigned int y = y0 * size_x_ + x0; y < yn * size_x_ + x0; y += size_x_)
    {
        memset(costmap_ + y, default_value_, len * sizeof(unsigned char));
    }
}

bool Costmap2D::copyCostmapWindow(const Costmap2D &map, double win_origin_x, double win_origin_y,
        double win_size_x, double win_size_y)
{
    // check for self windowing
    if (this == &map)
    {
        // ROS_ERROR("Cannot convert this costmap into a window of itself");
        return false;
    }
    // clean up old data
    deleteMaps();
    // compute the bounds of our new map
    unsigned int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
    if (!map.worldToMap(win_origin_x, win_origin_y, lower_left_x, lower_left_y) ||
        !map.worldToMap(win_origin_x + win_size_x, win_origin_y + win_size_y, upper_right_x, upper_right_y))
    {
        // ROS_ERROR("Cannot window a map that the window bounds don't fit inside of");
        return false;
    }
    size_x_ = upper_right_x - lower_left_x;
    size_y_ = upper_right_y - lower_left_y;
    resolution_ = map.resolution_;
    origin_x_ = win_origin_x;
    origin_y_ = win_origin_y;
    // initialize our various maps and reset markers for inflation
    initMaps(size_x_, size_y_);
    // copy the window of the static map and the costmap that we're taking
    copyMapRegion(map.costmap_, lower_left_x, lower_left_y, map.size_x_, costmap_, 0, 0, size_x_, size_x_, size_y_);
    return true;
}

Costmap2D &Costmap2D::operator=(const Costmap2D &map)
{
    // check for self assignement
    if (this == &map) return *this;
    // clean up old data
    deleteMaps();
    size_x_ = map.size_x_;
    size_y_ = map.size_y_;
    resolution_ = map.resolution_;
    origin_x_ = map.origin_x_;
    origin_y_ = map.origin_y_;
    // initialize our various maps
    initMaps(size_x_, size_y_);
    // copy the cost map
    memcpy(costmap_, map.costmap_, size_x_ * size_y_ * sizeof(unsigned char));
    return *this;
}

unsigned int Costmap2D::cellDistance(double world_dist)
{
    double cells_dist = max(0.0, ceil(world_dist / resolution_));
    return (unsigned int) cells_dist;
}

// +0.5 为了去除边界，防止计算到地图外面，(这里map是指像素坐标系，单位为int， world 是指/map世界坐标系)
void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double &wx, double &wy) const
{
    wx = origin_x_ + (mx + 0.5) * resolution_;
    wy = origin_y_ + (my + 0.5) * resolution_;
}

/**
* @brief  世界坐标系向map坐标系转换
* @param  wx 世界坐标系的x值
* @param  wy 世界坐标系的y值
* @param  mx 相关map坐标系下的x值
* @param  my 相关map坐标系下的y值
* @return True if the conversion was successful (legal bounds) false otherwise
*/
bool Costmap2D::worldToMap(double wx, double wy, unsigned int &mx, unsigned int &my) const
{
    if (wx < origin_x_ || wy < origin_y_) return false;
    mx = (int) ((wx - origin_x_) / resolution_);
    my = (int) ((wy - origin_y_) / resolution_);
    return mx < size_x_ && my < size_y_;
}

// 从世界坐标（wx, wy）向地图坐标(mx, my)转换,地图没有边界
void Costmap2D::worldToMapNoBounds(double wx, double wy, int &mx, int &my) const
{
    mx = (int) ((wx - origin_x_) / resolution_);
    my = (int) ((wy - origin_y_) / resolution_);
}

// 从世界坐标（wx, wy）向地图坐标(mx, my)转换,map中强制加入边界
void Costmap2D::worldToMapEnforceBounds(double wx, double wy, int &mx, int &my) const
{
    // Here we avoid doing any math to wx,wy before comparing them to the bounds,
    // so their values can go out to the max and min values of double floating point.
    if (wx < origin_x_) mx = 0;
    else if (wx >= resolution_ * size_x_ + origin_x_) mx = size_x_ - 1;
    else mx = (int) ((wx - origin_x_) / resolution_);
    if (wy < origin_y_) my = 0;
    else if (wy >= resolution_ * size_y_ + origin_y_) my = size_y_ - 1;
    else my = (int) ((wy - origin_y_) / resolution_);
}

void Costmap2D::updateOrigin(double new_origin_x, double new_origin_y)
{
    // project the new origin into the grid
    // Step 1: 将新原点投影到网格中
    int cell_ox, cell_oy;
    cell_ox = int((new_origin_x - origin_x_) / resolution_);
    cell_oy = int((new_origin_y - origin_y_) / resolution_);
    // Nothing to update 无需更新
    if (cell_ox == 0 && cell_oy == 0) return;
    // compute the associated world coordinates for the origin cell
    // because we want to keep things grid-aligned
    // Step 2: 计算原点单元格的关联世界坐标，因为我们希望保持网格对齐
    double new_grid_ox, new_grid_oy;
    new_grid_ox = origin_x_ + cell_ox * resolution_;
    new_grid_oy = origin_y_ + cell_oy * resolution_;
    // To save casting from unsigned int to int a bunch of times
    // Step 3: 将保存从无符号整型到int
    int size_x = size_x_;
    int size_y = size_y_;
    // we need to compute the overlap of the new and existing windows
    // Step 4: 我们需要计算新窗口和现有窗口的重叠
    int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
    lower_left_x = min(max(cell_ox, 0), size_x);
    lower_left_y = min(max(cell_oy, 0), size_y);
    upper_right_x = min(max(cell_ox + size_x, 0), size_x);
    upper_right_y = min(max(cell_oy + size_y, 0), size_y);
    unsigned int cell_size_x = upper_right_x - lower_left_x;
    unsigned int cell_size_y = upper_right_y - lower_left_y;
    // we need a map to store the obstacles in the window temporarily 新建局部地图
    unsigned char *local_map = new unsigned char[cell_size_x * cell_size_y];
    // copy the local window in the costmap to the local map
    // Step 5: 我们需要一张地图来暂时将障碍物存储在窗口中
    copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
    // now we'll set the costmap to be completely unknown if we track unknown space
    // Step 6: 现在，如果我们跟踪未知空间，我们将costmap设置为完全未知
    resetMaps();
    // update the origin with the appropriate world coordinates
    // Step 7: 使用适当的世界坐标更新原点
    origin_x_ = new_grid_ox;
    origin_y_ = new_grid_oy;
    // compute the starting cell location for copying data back in
    // Step 8: 计算用于将数据复制回的起始单元位置
    int start_x = lower_left_x - cell_ox;
    int start_y = lower_left_y - cell_oy;
    // now we want to copy the overlapping information back into the map, but in its new location
    // Step 9: 现在我们想将重叠的信息复制回地图中，但在其新位置。
    copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
    // Step 10: 删除局部地图make sure to clean up
    delete[] local_map;
}

bool Costmap2D::setConvexPolygonCost(const std::vector<geometry_msgs::Point> &polygon, unsigned char cost_value)
{
    // 机器人footprint的轮廓点全部转到地图坐标系下
    // we assume the polygon is given in the global_frame... we need to transform it to map coordinates
    std::vector<MapLocation> map_polygon;
    for (const auto & pt : polygon)
    {
        MapLocation loc;
        // ("Polygon lies outside map bounds, so we can't fill it");
        if (!worldToMap(pt.x, pt.y, loc.x, loc.y)) return false;
        map_polygon.push_back(loc);
    }
    std::vector<MapLocation> polygon_cells;
    // 通过机器人顶点坐标数组map_polygon得到多边形边缘及内部的全部cell，存放在polygon_cells中
    // get the cells that fill the polygon
    convexFillCells(map_polygon, polygon_cells);
    // 获取这些内部cell的index，再对地图costmap_ 遍历进行赋值操作
    // set the cost of those cells 把多边形边缘及内部的所有cell的cost设置为cost_value
    for (auto & polygon_cell : polygon_cells)
    {
        unsigned int index = getIndex(polygon_cell.x, polygon_cell.y);
        costmap_[index] = cost_value;
    }
    return true;
}

// 根据轮廓点，就能获得轮廓点连线的全部的边缘点
void Costmap2D::polygonOutlineCells(const std::vector<MapLocation> &polygon,
        std::vector<MapLocation> &polygon_cells)
{
    PolygonOutlineCells cell_gatherer(*this, costmap_, polygon_cells);
    for (unsigned int i = 0; i < polygon.size() - 1; ++i)
    {
        raytraceLine(cell_gatherer, polygon[i].x, polygon[i].y, polygon[i + 1].x, polygon[i + 1].y);
    }
    if (!polygon.empty())
    {
        unsigned int last_index = polygon.size() - 1;
        // we also need to close the polygon by going from the last point to the first
        raytraceLine(cell_gatherer, polygon[last_index].x, polygon[last_index].y, polygon[0].x, polygon[0].y);
    }
}

void Costmap2D::convexFillCells(const std::vector<MapLocation> &polygon, std::vector<MapLocation> &polygon_cells)
{
    // we need a minimum polygon of a triangle
    if (polygon.size() < 3) return;
    // first get the cells that make up the outline of the polygon
    // 通过给定的顶点提取多边形边上的cell
    polygonOutlineCells(polygon, polygon_cells);
    // quick bubble sort to sort points by x
    // 获得轮廓点之间连线的cell的列表。然后对这些边缘点做一次快速冒泡排序
    MapLocation swap;
    unsigned int i = 0;
    // 对边上的cell点的x做排序，使其按x坐标升序排列。
    while (i < polygon_cells.size() - 1)
    {
        if (polygon_cells[i].x > polygon_cells[i + 1].x)
        {
            swap = polygon_cells[i];
            polygon_cells[i] = polygon_cells[i + 1];
            polygon_cells[i + 1] = swap;
            if (i > 0) --i;
        }
        else ++i;
    }
    i = 0;
    MapLocation min_pt;
    MapLocation max_pt;
    unsigned int min_x = polygon_cells[0].x;
    unsigned int max_x = polygon_cells[polygon_cells.size() - 1].x;
    // walk through each column and mark cells inside the polygon
    // 遍历所有x，对每个相同的x，检查y，获得y最大和最小的polygon cell，将范围内的所有cell填充进polygon_cells，获得多边形边缘及内部的所有cell。
    for (unsigned int x = min_x; x <= max_x; ++x)
    {
        if (i >= polygon_cells.size() - 1) break;
        if (polygon_cells[i].y < polygon_cells[i + 1].y)
        {
            min_pt = polygon_cells[i];
            max_pt = polygon_cells[i + 1];
        }
        else
        {
            min_pt = polygon_cells[i + 1];
            max_pt = polygon_cells[i];
        }
        i += 2;
        // 操作完成后得到的polygon_cells 的cell都按照x坐标从小到大排序好了。然后开始沿着x轴，对每个相同的x，检查y值，获取y值最大的和y值最小的polygoncell
        while (i < polygon_cells.size() && polygon_cells[i].x == x)
        {
            if (polygon_cells[i].y < min_pt.y) min_pt = polygon_cells[i];
            else if (polygon_cells[i].y > max_pt.y) max_pt = polygon_cells[i];
            ++i;
        }
        // 最后将y最大的和y最小的整个列的所有cell全部都塞进polygon_cells去：
        MapLocation pt;
        // loop though cells in the column
        for (unsigned int y = min_pt.y; y <= max_pt.y; ++y)
        {
            pt.x = x;
            pt.y = y;
            polygon_cells.push_back(pt);
        }
    }
}

bool Costmap2D::saveMap(std::string file_name)
{
    FILE *fp = fopen(file_name.c_str(), "w");
    if (!fp) return false;
    fprintf(fp, "P2\n%u\n%u\n%u\n", size_x_, size_y_, 0xff);
    for (unsigned int iy = 0; iy < size_y_; iy++)
    {
        for (unsigned int ix = 0; ix < size_x_; ix++)
        {
            unsigned char cost = getCost(ix, iy);
            fprintf(fp, "%d ", cost);
        }
        fprintf(fp, "\n");
    }
    fclose(fp);
    return true;
}

}  // namespace costmap_2d
